/*
 * DbgUart.cc
 *
 *  Created on: Oct 14, 2016
 *      Author: zlk
 */

#include <xdc/std.h>
#include <xdc/runtime/System.h>
#include <stdio.h>

#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Mailbox.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <ti/sysbios/knl/Task.h>

#include <ti/sysbios/hal/Hwi.h>
#include <ti/sysbios/hal/Timer.h>

#include <inc/hw_sysctl.h>
#include <inc/hw_memmap.h>
#include <inc/hw_types.h>
#include <linemouse_chassis.h>
#include <ti/drivers/GPIO.h>

#include "../TskTop/DbgUart.h"
#include "../TskTop/TskTop.h"
#include "../TskMotor/TskMotor.h"
#include "TskLed.h"

namespace TskLed
{
	const int tskPrio = 11;
	const int tskStkSize = 2048;
	Task_Handle tsk;

	Semaphore_Handle SemLedFinish;
	Mailbox_Handle MbCmd;

	void ReverseLED0()
	{
		unsigned char rgb;

		rgb = (((GPIO_read(DBMOUSE_LED0_R)==DBMOUSE_LED_ON)?1:0) << 0)
             | (((GPIO_read(DBMOUSE_LED0_G)==DBMOUSE_LED_ON)?1:0) << 1)
			 | (((GPIO_read(DBMOUSE_LED0_B)==DBMOUSE_LED_ON)?1:0) << 2);

		if(rgb == TskTop::LedColor::Dark)
		{
			rgb = TskTop::LedColor::Yellow;
		}
		else
		{
			rgb = TskTop::LedColor::Dark;

		}
		GPIO_write(DBMOUSE_LED0_R, (rgb & 0x1) ? DBMOUSE_LED_ON : DBMOUSE_LED_OFF);
		GPIO_write(DBMOUSE_LED0_G, (rgb & 0x2) ? DBMOUSE_LED_ON : DBMOUSE_LED_OFF);
		GPIO_write(DBMOUSE_LED0_B, (rgb & 0x4) ? DBMOUSE_LED_ON : DBMOUSE_LED_OFF);
	}

	void ReverseLED1()
	{
		unsigned char rgb;

		rgb = (((GPIO_read(DBMOUSE_LED1_R)==DBMOUSE_LED_ON)?1:0) << 0)
             | (((GPIO_read(DBMOUSE_LED1_G)==DBMOUSE_LED_ON)?1:0) << 1)
			 | (((GPIO_read(DBMOUSE_LED1_B)==DBMOUSE_LED_ON)?1:0) << 2);

		if(rgb == TskTop::LedColor::Dark)
		{
			rgb = TskTop::LedColor::Blue;
		}
		else
		{
			rgb = TskTop::LedColor::Dark;

		}
		GPIO_write(DBMOUSE_LED1_R, (rgb & 0x1) ? DBMOUSE_LED_ON : DBMOUSE_LED_OFF);
		GPIO_write(DBMOUSE_LED1_G, (rgb & 0x2) ? DBMOUSE_LED_ON : DBMOUSE_LED_OFF);
		GPIO_write(DBMOUSE_LED1_B, (rgb & 0x4) ? DBMOUSE_LED_ON : DBMOUSE_LED_OFF);
	}

	void task(UArg arg0, UArg arg1)
	{
		int iPeriod=1000, iSteps;

		while(true)
		{
			iSteps = ((TskMotor::g_fSpeed>0) ? TskMotor::g_fSpeed : (-TskMotor::g_fSpeed)) / (1.479/50);
			iPeriod = 1000 - iSteps*100;
			iPeriod = (iPeriod>=100) ? iPeriod : 100;

			ReverseLED0();
			ReverseLED1();
			Task_sleep(iPeriod);
		}
	}

	void Init()
	{
		Task_Params tskParams;

		Task_Params_init(&tskParams);
		tskParams.priority = tskPrio;
		tskParams.stackSize = tskStkSize;
		tsk = Task_create(task, &tskParams, NULL);

	    if(tsk == NULL)
	        System_abort("TskLed failed.");
	}

}
